// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "sparse_system_helper.h"

#include <stdexcept>
#include <vector>

#include "g2o/core/io_helper.h"

namespace g2o {
namespace internal {

static std::vector<int> readIndices(const std::string& indeces) {
  std::stringstream tokens(indeces);

  int numElems = 0;
  tokens >> numElems;
  std::vector<int> result;
  for (int i = 0; i < numElems; ++i) {
    int number;
    tokens >> number;
    result.push_back(number);
  }
  return result;
}

// return the serialized sparse matrix
std::string sparseMatrixString() {
  std::stringstream aux;
  aux << "RBI : 12 3 6 9 12 15 18 21 24 27 30 33 36" << std::endl;
  aux << "CBI : 12 3 6 9 12 15 18 21 24 27 30 33 36" << std::endl;
  aux << "BLOCK : 0 0" << std::endl;
  aux << "2500 1.79023e-15 0" << std::endl;
  aux << "-1.79023e-15 2500 0" << std::endl;
  aux << "0 0 25000" << std::endl;
  aux << "BLOCK : 1 1" << std::endl;
  aux << "500 0 0" << std::endl;
  aux << "0 500 0" << std::endl;
  aux << "0 0 5000" << std::endl;
  aux << "BLOCK : 2 2" << std::endl;
  aux << "500 8.88178e-16 0" << std::endl;
  aux << "-8.88178e-16 500 0" << std::endl;
  aux << "0 0 5000" << std::endl;
  aux << "BLOCK : 3 3" << std::endl;
  aux << "500 4.44089e-16 0" << std::endl;
  aux << "-4.44089e-16 500 0" << std::endl;
  aux << "0 0 5000" << std::endl;
  aux << "BLOCK : 4 4" << std::endl;
  aux << "500 0 0" << std::endl;
  aux << "0 500 0" << std::endl;
  aux << "0 0 5000" << std::endl;
  aux << "BLOCK : 5 5" << std::endl;
  aux << "500 0 0" << std::endl;
  aux << "0 500 0" << std::endl;
  aux << "0 0 5000" << std::endl;
  aux << "BLOCK : 6 6" << std::endl;
  aux << "500 -8.88178e-16 0" << std::endl;
  aux << "8.88178e-16 500 0" << std::endl;
  aux << "0 0 5000" << std::endl;
  aux << "BLOCK : 0 7" << std::endl;
  aux << "-500 0 -2000" << std::endl;
  aux << "0 -500 -500" << std::endl;
  aux << "0 0 -5000" << std::endl;
  aux << "BLOCK : 7 7" << std::endl;
  aux << "500 0 2000" << std::endl;
  aux << "0 500 500" << std::endl;
  aux << "2000 500 13500" << std::endl;
  aux << "BLOCK : 0 8" << std::endl;
  aux << "-500 0 -1500" << std::endl;
  aux << "0 -500 -500" << std::endl;
  aux << "0 0 -5000" << std::endl;
  aux << "BLOCK : 8 8" << std::endl;
  aux << "500 0 1500" << std::endl;
  aux << "0 500 500" << std::endl;
  aux << "1500 500 10000" << std::endl;
  aux << "BLOCK : 0 9" << std::endl;
  aux << "-500 1.38778e-17 -1000" << std::endl;
  aux << "-1.38778e-17 -500 -500" << std::endl;
  aux << "0 0 -5000" << std::endl;
  aux << "BLOCK : 9 9" << std::endl;
  aux << "500 1.38778e-17 1000" << std::endl;
  aux << "-1.38778e-17 500 500" << std::endl;
  aux << "1000 500 7500" << std::endl;
  aux << "BLOCK : 0 10" << std::endl;
  aux << "-500 0 -500" << std::endl;
  aux << "0 -500 -500" << std::endl;
  aux << "0 0 -5000" << std::endl;
  aux << "BLOCK : 10 10" << std::endl;
  aux << "500 0 500" << std::endl;
  aux << "0 500 500" << std::endl;
  aux << "500 500 6000" << std::endl;
  aux << "BLOCK : 0 11" << std::endl;
  aux << "-500 1.77636e-15 -500" << std::endl;
  aux << "-1.77636e-15 -500 -500" << std::endl;
  aux << "0 0 -5000" << std::endl;
  aux << "BLOCK : 1 11" << std::endl;
  aux << "-500 0 -500" << std::endl;
  aux << "0 -500 -500" << std::endl;
  aux << "0 0 -5000" << std::endl;
  aux << "BLOCK : 2 11" << std::endl;
  aux << "-500 8.88178e-16 8.88178e-16" << std::endl;
  aux << "-8.88178e-16 -500 -500" << std::endl;
  aux << "0 0 -5000" << std::endl;
  aux << "BLOCK : 3 11" << std::endl;
  aux << "-500 4.44089e-16 500" << std::endl;
  aux << "-4.44089e-16 -500 -500" << std::endl;
  aux << "0 0 -5000" << std::endl;
  aux << "BLOCK : 4 11" << std::endl;
  aux << "-500 0 1000" << std::endl;
  aux << "0 -500 -500" << std::endl;
  aux << "0 0 -5000" << std::endl;
  aux << "BLOCK : 5 11" << std::endl;
  aux << "-500 0 1500" << std::endl;
  aux << "0 -500 -500" << std::endl;
  aux << "0 0 -5000" << std::endl;
  aux << "BLOCK : 6 11" << std::endl;
  aux << "-500 -8.88178e-16 1000" << std::endl;
  aux << "8.88178e-16 -500 -500" << std::endl;
  aux << "0 0 -5000" << std::endl;
  aux << "BLOCK : 11 11" << std::endl;
  aux << "4000 2.22045e-15 -3500" << std::endl;
  aux << "-2.22045e-15 4000 4000" << std::endl;
  aux << "-3500 4000 54500" << std::endl;
  return aux.str();
}

std::string denseInverseMatrixString() {
  std::stringstream aux;
  aux << "# rows: 36" << std::endl;
  aux << "# columns: 36" << std::endl;
  // clang-format off
  aux << "0.0047999999999999987 6.9908161755443545e-19 0.00040000000000000181 0.0027999999999999995 3.1776437161565264e-19 0.00040000000000000083 0.0023999999999999985 3.1776437161565264e-19 0.00040000000000000083 0.0019999999999999974 3.1776437161565264e-19 0.00040000000000000083 0.0015999999999999968 3.1776437161565264e-19 0.00040000000000000083 0.0011999999999999958 3.1776437161565264e-19 0.00040000000000000083 0.0015999999999999968 3.1776437161565264e-19 0.00040000000000000083 0.0031999999999999893 -0.00040000000000000137 0.00040000000000000213 0.0035999999999999921 -0.00040000000000000127 0.00040000000000000192 0.0039999999999999931 -0.00040000000000000127 0.00040000000000000192 0.0043999999999999959 -0.00040000000000000116 0.00040000000000000186 0.0023999999999999989 -0.00040000000000000051 0.00040000000000000083" << std::endl;
  aux << "6.9908161755443545e-19 0.0039999999999999957 7.1333868948638727e-19 6.9908161755443391e-19 0.0019999999999999974 3.4954080877721633e-19 3.4954080877721763e-19 0.0019999999999999974 3.4954080877721633e-19 1.3481509610710651e-33 0.0019999999999999974 3.4954080877721633e-19 -3.4954080877721484e-19 0.0019999999999999974 3.4954080877721633e-19 -6.9908161755443121e-19 0.0019999999999999974 3.4954080877721633e-19 -3.4954080877721484e-19 0.0019999999999999974 3.4954080877721633e-19 -2.5180710211002832e-18 0.0039999999999999949 8.042881596636798e-19 -1.7137828614366046e-18 0.0039999999999999949 8.0428815966367999e-19 -9.0949470177292492e-19 0.0039999999999999949 8.0428815966367999e-19 -1.0520654210924454e-19 0.0039999999999999949 8.0428815966367999e-19 3.4954080877721768e-19 0.001999999999999997 3.4954080877721633e-19" << std::endl;
  aux << "0.00040000000000000181 7.1333868948638727e-19 0.00040000000000000029 0.00040000000000000105 3.4078112616241102e-19 0.00020000000000000023 0.00020000000000000085 3.4078112616241102e-19 0.00020000000000000023 6.7762635780344027e-19 3.4078112616241102e-19 0.00020000000000000023 -0.00019999999999999949 3.4078112616241102e-19 0.00020000000000000023 -0.00039999999999999975 3.4078112616241102e-19 0.00020000000000000023 -0.00019999999999999949 3.4078112616241102e-19 0.00020000000000000023 -0.0011999999999999986 -0.00039999999999999942 0.00040000000000000018 -0.00079999999999999906 -0.00039999999999999953 0.00040000000000000029 -0.00039999999999999904 -0.00039999999999999953 0.00040000000000000029 1.463672932855431e-18 -0.00039999999999999953 0.00040000000000000029 0.0002000000000000009 -0.00019999999999999987 0.00020000000000000023" << std::endl;
  aux << "0.0027999999999999995 6.9908161755443391e-19 0.00040000000000000105 0.0047999999999999996 3.1776437161565192e-19 0.00040000000000000051 0.0023999999999999989 3.1776437161565192e-19 0.00040000000000000051 0.0019999999999999979 3.1776437161565192e-19 0.00040000000000000051 0.0015999999999999979 3.1776437161565192e-19 0.00040000000000000051 0.0011999999999999971 3.1776437161565192e-19 0.00040000000000000051 0.0015999999999999979 3.1776437161565192e-19 0.00040000000000000051 0.0011999999999999945 -0.00040000000000000051 0.00040000000000000116 0.0015999999999999953 -0.00040000000000000045 0.0004000000000000011 0.0019999999999999957 -0.00040000000000000045 0.0004000000000000011 0.0023999999999999976 -0.0004000000000000004 0.00040000000000000105 0.0023999999999999994 -0.00040000000000000024 0.00040000000000000051" << std::endl;
  aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0039999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl;
  aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00040000000000000018 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl;
  aux << "0.0023999999999999985 3.4954080877721763e-19 0.00020000000000000085 0.0023999999999999989 1.588821858078263e-19 0.00020000000000000042 0.0041999999999999989 1.588821858078263e-19 0.00020000000000000042 0.0019999999999999974 1.588821858078263e-19 0.00020000000000000042 0.0017999999999999973 1.588821858078263e-19 0.00020000000000000042 0.0015999999999999968 1.588821858078263e-19 0.00020000000000000042 0.0017999999999999973 1.588821858078263e-19 0.00020000000000000042 0.0015999999999999938 -0.00020000000000000069 0.00020000000000000101 0.0017999999999999952 -0.00020000000000000061 0.00020000000000000093 0.0019999999999999957 -0.00020000000000000061 0.00020000000000000093 0.0021999999999999971 -0.00020000000000000052 0.0002000000000000009 0.0021999999999999984 -0.00020000000000000025 0.00020000000000000042" << std::endl;
  aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0039999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl;
  aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00040000000000000018 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl;
  aux << "0.0019999999999999974 1.3481509610710651e-33 6.7762635780344027e-19 0.0019999999999999979 6.98149604840373e-34 2.9815559743351372e-19 0.0019999999999999974 6.98149604840373e-34 2.9815559743351372e-19 0.0039999999999999975 6.98149604840373e-34 2.9815559743351372e-19 0.001999999999999997 6.98149604840373e-34 2.9815559743351372e-19 0.0019999999999999966 6.98149604840373e-34 2.9815559743351372e-19 0.001999999999999997 6.98149604840373e-34 2.9815559743351372e-19 0.0019999999999999931 -8.9446679230054116e-19 8.6736173798840355e-19 0.0019999999999999948 -7.8604657505199071e-19 7.8604657505199071e-19 0.0019999999999999953 -7.8604657505199071e-19 7.8604657505199071e-19 0.0019999999999999961 -7.0473141211557788e-19 7.3183646642771549e-19 0.0019999999999999979 -2.7105054312137611e-19 2.9815559743351372e-19" << std::endl;
  aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0039999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl;
  aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00040000000000000018 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl;
  aux << "0.0015999999999999968 -3.4954080877721484e-19 -0.00019999999999999949 0.0015999999999999979 -1.588821858078249e-19 -0.00019999999999999979 0.0017999999999999973 -1.588821858078249e-19 -0.00019999999999999979 0.001999999999999997 -1.588821858078249e-19 -0.00019999999999999979 0.0041999999999999971 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999968 -1.588821858078249e-19 -0.00019999999999999979 0.0021999999999999967 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999933 0.00019999999999999887 -0.00019999999999999925 0.0021999999999999949 0.00019999999999999903 -0.00019999999999999936 0.0019999999999999953 0.00019999999999999903 -0.00019999999999999936 0.0017999999999999956 0.00019999999999999912 -0.00019999999999999939 0.0017999999999999976 0.00019999999999999966 -0.00019999999999999979" << std::endl;
  aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0039999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl;
  aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00040000000000000018 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl;
  aux << "0.0011999999999999958 -6.9908161755443121e-19 -0.00039999999999999975 0.0011999999999999971 -3.1776437161565057e-19 -0.00039999999999999996 0.0015999999999999968 -3.1776437161565057e-19 -0.00039999999999999996 0.0019999999999999966 -3.1776437161565057e-19 -0.00039999999999999996 0.0023999999999999968 -3.1776437161565057e-19 -0.00039999999999999996 0.0047999999999999961 -3.1776437161565057e-19 -0.00039999999999999996 0.0023999999999999968 -3.1776437161565057e-19 -0.00039999999999999996 0.002799999999999993 0.00039999999999999872 -0.00039999999999999942 0.002399999999999995 0.00039999999999999899 -0.00039999999999999964 0.0019999999999999948 0.00039999999999999899 -0.00039999999999999964 0.0015999999999999951 0.00039999999999999904 -0.00039999999999999964 0.001599999999999997 0.00039999999999999969 -0.00039999999999999996" << std::endl;
  aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0039999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl;
  aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00040000000000000018 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl;
  aux << "0.0015999999999999968 -3.4954080877721484e-19 -0.00019999999999999949 0.0015999999999999979 -1.588821858078249e-19 -0.00019999999999999979 0.0017999999999999973 -1.588821858078249e-19 -0.00019999999999999979 0.001999999999999997 -1.588821858078249e-19 -0.00019999999999999979 0.0021999999999999967 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999968 -1.588821858078249e-19 -0.00019999999999999979 0.0041999999999999971 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999933 0.00019999999999999887 -0.00019999999999999925 0.0021999999999999949 0.00019999999999999903 -0.00019999999999999936 0.0019999999999999953 0.00019999999999999903 -0.00019999999999999936 0.0017999999999999956 0.00019999999999999912 -0.00019999999999999939 0.0017999999999999976 0.00019999999999999966 -0.00019999999999999979" << std::endl;
  aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0039999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl;
  aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00040000000000000018 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl;
  aux << "0.0031999999999999893 -2.5180710211002832e-18 -0.0011999999999999986 0.0011999999999999945 -1.2272590733885761e-18 -0.00039999999999999986 0.0015999999999999938 -1.2272590733885761e-18 -0.00039999999999999986 0.0019999999999999931 -1.2272590733885761e-18 -0.00039999999999999986 0.0023999999999999933 -1.2272590733885761e-18 -0.00039999999999999986 0.002799999999999993 -1.2272590733885761e-18 -0.00039999999999999986 0.0023999999999999933 -1.2272590733885761e-18 -0.00039999999999999986 0.013199999999999977 0.0019999999999999944 -0.0019999999999999974 0.0067999999999999849 0.0011999999999999958 -0.0011999999999999986 0.0055999999999999852 0.0011999999999999958 -0.0011999999999999986 0.0043999999999999873 0.001199999999999996 -0.0011999999999999986 0.0015999999999999942 0.00039999999999999866 -0.00039999999999999986" << std::endl;
  aux << "-0.00040000000000000137 0.0039999999999999949 -0.00039999999999999942 -0.00040000000000000051 0.0019999999999999966 -0.00019999999999999982 -0.00020000000000000069 0.0019999999999999966 -0.00019999999999999982 -8.9446679230054116e-19 0.0019999999999999966 -0.00019999999999999982 0.00019999999999999887 0.0019999999999999966 -0.00019999999999999982 0.00039999999999999872 0.0019999999999999966 -0.00019999999999999982 0.00019999999999999887 0.0019999999999999966 -0.00019999999999999982 0.0019999999999999944 0.0065999999999999939 -0.00059999999999999908 0.00079999999999999668 0.0043999999999999933 -0.00039999999999999931 0.00039999999999999747 0.0043999999999999933 -0.00039999999999999931 -2.0328790734103208e-18 0.0043999999999999933 -0.00039999999999999931 -0.00020000000000000074 0.0021999999999999967 -0.00019999999999999982" << std::endl;
  aux << "0.00040000000000000213 8.042881596636798e-19 0.00040000000000000018 0.00040000000000000116 3.8625586125105733e-19 0.0002000000000000002 0.00020000000000000101 3.8625586125105733e-19 0.0002000000000000002 8.6736173798840355e-19 3.8625586125105733e-19 0.0002000000000000002 -0.00019999999999999925 3.8625586125105733e-19 0.0002000000000000002 -0.00039999999999999942 3.8625586125105733e-19 0.0002000000000000002 -0.00019999999999999925 3.8625586125105733e-19 0.0002000000000000002 -0.0019999999999999974 -0.00059999999999999908 0.00059999999999999995 -0.00079999999999999852 -0.00039999999999999931 0.00040000000000000013 -0.00039999999999999845 -0.00039999999999999931 0.00040000000000000013 1.9244588561617704e-18 -0.00039999999999999931 0.00040000000000000013 0.00020000000000000107 -0.00019999999999999982 0.0002000000000000002" << std::endl;
  aux << "0.0035999999999999921 -1.7137828614366046e-18 -0.00079999999999999906 0.0015999999999999953 -8.4100321213751965e-19 -0.00019999999999999996 0.0017999999999999952 -8.4100321213751965e-19 -0.00019999999999999996 0.0019999999999999948 -8.4100321213751965e-19 -0.00019999999999999996 0.0021999999999999949 -8.4100321213751965e-19 -0.00019999999999999996 0.002399999999999995 -8.4100321213751965e-19 -0.00019999999999999996 0.0021999999999999949 -8.4100321213751965e-19 -0.00019999999999999996 0.0067999999999999849 0.00079999999999999668 -0.00079999999999999852 0.0097999999999999893 0.001399999999999997 -0.0013999999999999989 0.0051999999999999894 0.00079999999999999711 -0.00079999999999999895 0.0043999999999999899 0.00079999999999999722 -0.00079999999999999906 0.0017999999999999954 0.00019999999999999912 -0.00019999999999999996" << std::endl;
  aux << "-0.00040000000000000127 0.0039999999999999949 -0.00039999999999999953 -0.00040000000000000045 0.0019999999999999966 -0.00019999999999999987 -0.00020000000000000061 0.0019999999999999966 -0.00019999999999999987 -7.8604657505199071e-19 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.00039999999999999899 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.0011999999999999958 0.0043999999999999933 -0.00039999999999999931 0.001399999999999997 0.0065999999999999939 -0.00059999999999999941 0.00039999999999999785 0.0043999999999999933 -0.00039999999999999942 -1.7889335846010823e-18 0.0043999999999999933 -0.00039999999999999942 -0.00020000000000000066 0.0021999999999999967 -0.00019999999999999987" << std::endl;
  aux << "0.00040000000000000192 8.0428815966367999e-19 0.00040000000000000029 0.0004000000000000011 3.8625586125105738e-19 0.0002000000000000002 0.00020000000000000093 3.8625586125105738e-19 0.0002000000000000002 7.8604657505199071e-19 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.00039999999999999964 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.0011999999999999986 -0.00039999999999999931 0.00040000000000000013 -0.0013999999999999989 -0.00059999999999999941 0.00060000000000000027 -0.00039999999999999877 -0.00039999999999999942 0.00040000000000000029 1.6263032587282567e-18 -0.00039999999999999942 0.00040000000000000029 0.00020000000000000099 -0.00019999999999999985 0.0002000000000000002" << std::endl;
  aux << "0.0039999999999999931 -9.0949470177292492e-19 -0.00039999999999999904 0.0019999999999999957 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999957 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999953 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999953 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999948 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999953 -4.5474735088646227e-19 2.1684043449710089e-19 0.0055999999999999852 0.00039999999999999747 -0.00039999999999999845 0.0051999999999999894 0.00039999999999999785 -0.00039999999999999877 0.0075999999999999896 0.00079999999999999754 -0.00079999999999999874 0.0043999999999999907 0.0003999999999999979 -0.00039999999999999888 0.0019999999999999961 -6.7762635780344027e-19 2.1684043449710089e-19" << std::endl;
  aux << "-0.00040000000000000127 0.0039999999999999949 -0.00039999999999999953 -0.00040000000000000045 0.0019999999999999966 -0.00019999999999999987 -0.00020000000000000061 0.0019999999999999966 -0.00019999999999999987 -7.8604657505199071e-19 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.00039999999999999899 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.0011999999999999958 0.0043999999999999933 -0.00039999999999999931 0.00079999999999999711 0.0043999999999999933 -0.00039999999999999942 0.00079999999999999754 0.0065999999999999939 -0.00059999999999999941 -1.7889335846010823e-18 0.0043999999999999933 -0.00039999999999999942 -0.00020000000000000066 0.0021999999999999967 -0.00019999999999999987" << std::endl;
  aux << "0.00040000000000000192 8.0428815966367999e-19 0.00040000000000000029 0.0004000000000000011 3.8625586125105738e-19 0.0002000000000000002 0.00020000000000000093 3.8625586125105738e-19 0.0002000000000000002 7.8604657505199071e-19 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.00039999999999999964 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.0011999999999999986 -0.00039999999999999931 0.00040000000000000013 -0.00079999999999999895 -0.00039999999999999942 0.00040000000000000029 -0.00079999999999999874 -0.00059999999999999941 0.00060000000000000027 1.6263032587282567e-18 -0.00039999999999999942 0.00040000000000000029 0.00020000000000000099 -0.00019999999999999985 0.0002000000000000002" << std::endl;
  aux << "0.0043999999999999959 -1.0520654210924454e-19 1.463672932855431e-18 0.0023999999999999976 -6.8491489635404718e-20 0.00020000000000000058 0.0021999999999999971 -6.8491489635404718e-20 0.00020000000000000058 0.0019999999999999961 -6.8491489635404718e-20 0.00020000000000000058 0.0017999999999999956 -6.8491489635404718e-20 0.00020000000000000058 0.0015999999999999951 -6.8491489635404718e-20 0.00020000000000000058 0.0017999999999999956 -6.8491489635404718e-20 0.00020000000000000058 0.0043999999999999873 -2.0328790734103208e-18 1.9244588561617704e-18 0.0043999999999999899 -1.7889335846010823e-18 1.6263032587282567e-18 0.0043999999999999907 -1.7889335846010823e-18 1.6263032587282567e-18 0.006599999999999993 0.00019999999999999819 -0.00019999999999999836 0.0021999999999999971 -0.00020000000000000066 0.00020000000000000058" << std::endl;
  aux << "-0.00040000000000000116 0.0039999999999999949 -0.00039999999999999953 -0.0004000000000000004 0.0019999999999999966 -0.00019999999999999987 -0.00020000000000000052 0.0019999999999999966 -0.00019999999999999987 -7.0473141211557788e-19 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999912 0.0019999999999999966 -0.00019999999999999987 0.00039999999999999904 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999912 0.0019999999999999966 -0.00019999999999999987 0.001199999999999996 0.0043999999999999933 -0.00039999999999999931 0.00079999999999999722 0.0043999999999999933 -0.00039999999999999942 0.0003999999999999979 0.0043999999999999933 -0.00039999999999999942 0.00019999999999999819 0.0065999999999999939 -0.00059999999999999941 -0.00020000000000000058 0.0021999999999999967 -0.00019999999999999987" << std::endl;
  aux << "0.00040000000000000186 8.0428815966367999e-19 0.00040000000000000029 0.00040000000000000105 3.8625586125105738e-19 0.0002000000000000002 0.0002000000000000009 3.8625586125105738e-19 0.0002000000000000002 7.3183646642771549e-19 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999939 3.8625586125105738e-19 0.0002000000000000002 -0.00039999999999999964 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999939 3.8625586125105738e-19 0.0002000000000000002 -0.0011999999999999986 -0.00039999999999999931 0.00040000000000000013 -0.00079999999999999906 -0.00039999999999999942 0.00040000000000000029 -0.00039999999999999888 -0.00039999999999999942 0.00040000000000000029 -0.00019999999999999836 -0.00059999999999999941 0.00060000000000000027 0.00020000000000000093 -0.00019999999999999985 0.0002000000000000002" << std::endl;
  aux << "0.0023999999999999989 3.4954080877721768e-19 0.0002000000000000009 0.0023999999999999994 1.5888218580782632e-19 0.00020000000000000044 0.0021999999999999984 1.5888218580782632e-19 0.00020000000000000044 0.0019999999999999979 1.5888218580782632e-19 0.00020000000000000044 0.0017999999999999976 1.5888218580782632e-19 0.00020000000000000044 0.001599999999999997 1.5888218580782632e-19 0.00020000000000000044 0.0017999999999999976 1.5888218580782632e-19 0.00020000000000000044 0.0015999999999999942 -0.00020000000000000074 0.00020000000000000107 0.0017999999999999954 -0.00020000000000000066 0.00020000000000000099 0.0019999999999999961 -0.00020000000000000066 0.00020000000000000099 0.0021999999999999971 -0.00020000000000000058 0.00020000000000000093 0.0021999999999999988 -0.00020000000000000028 0.00020000000000000044" << std::endl;
  aux << "-0.00040000000000000051 0.001999999999999997 -0.00019999999999999987 -0.00040000000000000024 0.0019999999999999983 -0.00020000000000000001 -0.00020000000000000025 0.0019999999999999983 -0.00020000000000000001 -2.7105054312137611e-19 0.0019999999999999983 -0.00020000000000000001 0.00019999999999999966 0.0019999999999999983 -0.00020000000000000001 0.00039999999999999969 0.0019999999999999983 -0.00020000000000000001 0.00019999999999999966 0.0019999999999999983 -0.00020000000000000001 0.00039999999999999866 0.0021999999999999967 -0.00019999999999999982 0.00019999999999999912 0.0021999999999999967 -0.00019999999999999985 -6.7762635780344027e-19 0.0021999999999999967 -0.00019999999999999985 -0.00020000000000000066 0.0021999999999999967 -0.00019999999999999985 -0.00020000000000000028 0.0021999999999999984 -0.00020000000000000001" << std::endl;
  aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl;
  // clang-format on
  return aux.str();
}

void fillTestMatrix(g2o::SparseBlockMatrixX& A) {
  std::stringstream input(sparseMatrixString());
  std::string token;

  // reading RBI
  input >> token >> token;
  std::getline(input, token);
  std::vector<int> rowBlockIndices = readIndices(token);
  // reading CBI
  input >> token >> token;
  std::getline(input, token);
  std::vector<int> colBlockIndices = readIndices(token);

  // allocating the matrix and read the elements of the matrix
  A.clear(true);
  A.rowBlockIndices() = rowBlockIndices;
  A.colBlockIndices() = colBlockIndices;
  A.blockCols().resize(colBlockIndices.size());

  while (input >> token) {
    if (token != "BLOCK") {
      throw std::logic_error("Expected BLOCK as token");
    }
    input >> token;
    int ri, ci;
    input >> ri >> ci;
    auto sparseBlock = A.block(ri, ci, true);
    sparseBlock->resize(A.rowsOfBlock(ri), A.colsOfBlock(ci));
    for (int rr = 0; rr < sparseBlock->rows(); rr++) {
      for (int cc = 0; cc < sparseBlock->cols(); cc++) {
        double value;
        input >> value;
        (*sparseBlock)(rr, cc) = value;
      }
    }
  }
}

g2o::MatrixX createTestMatrixInverse() {
  std::stringstream aux(denseInverseMatrixString());
  std::string token;

  // reading number of rows and columns
  int numRows = 0;
  aux >> token >> token >> numRows;
  int numCols = 0;
  aux >> token >> token >> numCols;

  g2o::MatrixX result(numRows, numCols);
  for (int r = 0; r < numRows; ++r) {
    for (int c = 0; c < numCols; ++c) {
      double value = NAN;
      aux >> value;
      result(r, c) = value;
    }
  }
  return result;
}

g2o::VectorX createTestVectorB() {
  g2o::VectorX result;
  result.resize(12 * 3);
  int idx = 0;
  result(idx++) = 6.19602;
  result(idx++) = -53.8323;
  result(idx++) = 206.224;
  result(idx++) = 57.225;
  result(idx++) = -13.98;
  result(idx++) = -58.65;
  result(idx++) = 19.87;
  result(idx++) = -12.0698;
  result(idx++) = -44.5;
  result(idx++) = -7.5935;
  result(idx++) = 22.79;
  result(idx++) = -34.15;
  result(idx++) = 2.71;
  result(idx++) = 47.155;
  result(idx++) = -20.35;
  result(idx++) = -23.688;
  result(idx++) = 8.295;
  result(idx++) = -162.7;
  result(idx++) = -1.376;
  result(idx++) = -32.535;
  result(idx++) = 65.45;
  result(idx++) = -3.00264;
  result(idx++) = 3.27685;
  result(idx++) = -41.2837;
  result(idx++) = 41.851;
  result(idx++) = -3.65832;
  result(idx++) = 137.045;
  result(idx++) = -6.67126;
  result(idx++) = 18.5169;
  result(idx++) = 4.47434;
  result(idx++) = -23.5981;
  result(idx++) = 12.2319;
  result(idx++) = -94.4162;
  result(idx++) = -95.8125;
  result(idx++) = 34.0934;
  result(idx++) = 87.0203;
  return result;
}

g2o::VectorX createTestVectorX() {
  g2o::VectorX result;
  result.resize(12 * 3);
  int idx = 0;
  result(idx++) = -0.03134963;
  result(idx++) = 0.01363732;
  result(idx++) = 0.02445500;
  result(idx++) = 0.05355033;
  result(idx++) = 0.03260726;
  result(idx++) = -0.00828985;
  result(idx++) = -0.02459981;
  result(idx++) = 0.03642766;
  result(idx++) = -0.00545985;
  result(idx++) = -0.08296696;
  result(idx++) = 0.10614726;
  result(idx++) = -0.00338985;
  result(idx++) = -0.06580011;
  result(idx++) = 0.15487726;
  result(idx++) = -0.00062985;
  result(idx++) = -0.12203625;
  result(idx++) = 0.07715726;
  result(idx++) = -0.02909985;
  result(idx++) = -0.07397211;
  result(idx++) = -0.00450274;
  result(idx++) = 0.01653015;
  result(idx++) = -0.10913493;
  result(idx++) = 0.00224601;
  result(idx++) = 0.01794501;
  result(idx++) = -0.03010283;
  result(idx++) = -0.02116439;
  result(idx++) = 0.02748507;
  result(idx++) = -0.09332214;
  result(idx++) = 0.02635612;
  result(idx++) = 0.02431500;
  result(idx++) = -0.08639083;
  result(idx++) = 0.03025612;
  result(idx++) = 0.00784500;
  result(idx++) = -0.06433981;
  result(idx++) = 0.05712711;
  result(idx++) = 0.00344015;
  return result;
}

}  // namespace internal
}  // namespace g2o
